#include "uart.h"
#include <stdio.h>
using namespace std;

UART::UART():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,57600);
    _to = Timeout::simpleTimeout(100);

    _rect_pub = nh.advertise<unionsys_msgs::TrackRectangle>("track_rect",5,true);
    //    _gimbal_vel_sub = nh.subscribe("staple_to_gimbal",100,&UART::GimbalVelCallback,this);

}

//void UART::GimbalVelCallback(const unionsys_msgs::Gimbal_PidVel_cmd& msg)
//{
//    int v_yaw = msg.v_yaw;
//    int v_pitch = msg.v_pitch;

//    char* vel_yaw = new char(8);
//    char* vel_pitch = new char(8);
//    char* res = new char(8);
//    char* res_pitch = new char(8);
//    sprintf(vel_yaw,"%X",v_yaw);
//    sprintf(vel_pitch,"%X",v_pitch);
//    if(v_yaw>0)
//    {
//        if(v_yaw<16)
//        {
//            res[0] = '0';
//            res[1] = vel_yaw[0];
//        }
//        else
//        {
//            res[0] = vel_yaw[0];
//            res[1] = vel_yaw[1];
//        }
//    }
//    else
//    {
//        if(-v_yaw<16)
//        {
//            res[0] = '0';
//            res[1] = vel_yaw[7];
//        }
//        else
//        {
//            res[0] = vel_yaw[6];
//            res[1] = vel_yaw[7];
//        }
//    }


//    if(v_pitch>0)
//    {
//        if(v_pitch<16)
//        {
//            res_pitch[0] = '0';
//            res_pitch[1] = vel_pitch[0];
//        }
//        else
//        {
//            res_pitch[0] = vel_pitch[0];
//            res_pitch[1] = vel_pitch[1];
//        }
//    }
//    else
//    {
//        if(-v_pitch<16)
//        {
//            res_pitch[0] = '0';
//            res_pitch[1] = vel_pitch[7];
//        }
//        else
//        {
//            res_pitch[0] = vel_pitch[6];
//            res_pitch[1] = vel_pitch[7];
//        }
//    }

//    char temp[16] = {'#','t','p','U','G','4','w','G','S','M',res[0],res[1],res_pitch[0],res_pitch[1]};
//    char* package = StringToHex_s(temp,14);
//    temp[14] = package[0];
//    temp[15] = package[1];


//    write(temp);
//}


string UART::DecIntToHexStr(uint8_t num)
{
    string str;
    uint8_t Temp = num / 16;
    int left = num % 16;
    if (Temp > 0)
        str += DecIntToHexStr(Temp);
    if (left < 10)
        str += (left + '0');
    else
        str += ('A' + left - 10);
    return str;
}



char *UART::StringToHex_s(char *String, int Inlen)
{
    if (String == NULL || String == nullptr) { return nullptr; }

    int BufLen = Inlen * 2 + 1;

    char *Buf = new char[BufLen];
    memset(Buf, 0, BufLen);

    char buf[3] = { 0 };

    for (int i = 0; i < Inlen; i++)
    {

        sprintf(buf, "%02X", (unsigned char)String[i]);
        (Buf + (i * 2))[0] = buf[0];
        (Buf + (i * 2))[1] = buf[1];
    }
    return Buf;
}

void UART::OpenSerial()
{
    try {
        this->open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }

    if(this->isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return;
    }
}

uint8_t UART::CrcCheck(std::vector<uint8_t> buff,int len)
{
    uint8_t crc = 0;

    for(uint8_t i=0;i<len;i++)
    {
        crc += buff[i];
    }

    return crc;
}

char* UART::CrcCheckForGimbal(char* ch,int len)
{
    int crc = 0;
    for(uint8_t i=0;i<len;i++)
    {
        crc += ch[i];
    }
    char* res = new char(8);
    sprintf(res,"%X",crc);

    return res;
}

void UART::RunMain()
{
    uint8_t package[256];

    uint8_t rx_head = 0;
    uint8_t rx_wr_index = 0;
    uint8_t b_rx_over = 0;
    serial_process_t _serial_process;

    OpenSerial();
    ros::Rate loop_rate(10);
    while(ros::ok())
    {

        size_t n = this->available();

        uint8_t buffer[256];
        n = this->read(buffer,n);//must! n<=256
        uint8_t b[256];
        for(uint8_t i=0;i<n;i++)
        {
            b[i] = buffer[i];
        }

        for(uint8_t i=0;i<n;i++)
        {
            if(b[i]==0x11)
            {
                rx_head = 1;
                package[rx_wr_index++] = b[i];
            }
            else if(b[i]==0x27)
            {
                if(rx_head)
                {
                    rx_wr_index = 0;
                    b_rx_over = 0;
                    //                qDebug()<<"rx_head";
                }
                else
                {
                    package[rx_wr_index++] = b[i];
                    rx_head = 0;
                }

            }
            else
            {
                package[rx_wr_index++] = b[i];
                rx_head = 0;
                if(rx_wr_index>11)
                {
                    if((rx_wr_index+2) == package[2] && package[0]==0x01 && package[1]==0x37)
                    {

                        std::cout<<"xxx"<<std::endl;
                        b_rx_over = 1;

                        _serial_process.rx_buffer.clear();
                        _serial_process.rx_buffer.push_back(0x11);
                        _serial_process.rx_buffer.push_back(0x27);


                        for(uint8_t i=2; i<package[2];i++)
                        {
                            _serial_process.rx_buffer[i] = package[i-2];
                        }

                        _serial_process.index = package[2];
                        _serial_process.rx_over = 1;
                        memset(package,0,256);
                    }
                }
            }
            if(rx_wr_index>200)
                rx_wr_index=200;
        }

        if(_serial_process.rx_over==1)
        {
            std::vector<uint8_t> buffer;
            for(uint8_t i=0;i<_serial_process.index;i++)
            {

                buffer.push_back(_serial_process.rx_buffer[i]);
            }
            std::cout<<(int)buffer[0]<<std::endl;
            if(buffer[0]==0x11 && buffer[1]==0x27 && buffer[2]==0x01 && buffer[3]==0x37)
            {
                std::cout<<"ccc"<<std::endl;
                //crc check
                uint8_t crc = CrcCheck(buffer,13);
                if(crc == buffer[_serial_process.index-1])
                {
                    uint8_t temp[14];
                    for(uint8_t i=0;i<14;i++)
                    {
                        temp[i] = buffer[i];
                    }
                    memcpy(&_rect,temp+5,8);

                    unionsys_msgs::TrackRectangle msg;
                    msg.xmin = _rect.xmin;
                    msg.ymin = _rect.ymin;
                    msg.xmax = _rect.xmax;
                    msg.ymax = _rect.ymax;
                    _rect_pub.publish(msg);
                }
            }

            _serial_process.rx_over = 0;
        }


        ros::spinOnce();
        loop_rate.sleep();
    }
}
